# -*- coding:utf-8 -*-
# @Time: 2020/11/14 11:16
# @Author: xiaolong
# @File: priority_search.py

from queue import PriorityQueue


# 单元格类型
# 0 - 路，1 - 墙，2-走过的路，3-死胡同
class CellType:
    ROAD = 0
    WALL = 1
    WALKED = 2
    DEAD = 3


# 墙的方向
class Direction:
    LEFT = 0,
    UP = 1,
    RIGHT = 2,
    DOWN = 3,


def mark_walked(maze, pos):
    maze[pos[0]][pos[1]] = CellType.WALKED


def mark_dead(maze, pos):
    maze[pos[0]][pos[1]] = CellType.DEAD


def valid(maze, x, y):
    if x < 0 or y < 0:
        return False
    if x >= len(maze) or y >= len(maze):
        return False
    val = maze[x][y]
    if val == CellType.WALL or val == CellType.DEAD:
        return False
    return True


fx = [[1, 0], [0, 1], [-1, 0], [0, -1]]


def neighbors(maze, p):
    nod = []
    for c in fx:
        x = p[0] + c[0]
        y = p[1] + c[1]
        if valid(maze, x, y):
            nod.append((x, y))
    return nod


def heuristic(p, q):
    return abs(p[0] - q[0]) + abs(p[1] - q[1])


came_from = dict()
cost_so_far = dict()


def make_path(maze, p, pos):
    mark_walked(maze, p)
    while p != pos:
        p = came_from[p]
        mark_walked(maze, p)


def solve_maze(maze, pos, end, callback):
    # 优先队列
    pq = PriorityQueue()
    pq.put((0, pos))
    came_from.clear()
    cost_so_far.clear()
    came_from[pos] = None
    cost_so_far[pos] = 0
    while not pq.empty():
        current = pq.get()[1]
        # 标记走过
        mark_dead(maze, current)
        if current == end:
            make_path(maze, end, pos)
            break
        for nxt in neighbors(maze, current):
            new_cost = cost_so_far[current] + 1
            if nxt not in cost_so_far or new_cost < cost_so_far[nxt]:
                cost_so_far[nxt] = new_cost
                priority = new_cost + heuristic(nxt, end)
                pq.put((priority, nxt))
                came_from[nxt] = current
    callback(maze)
